package com.grt192.sensor;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.Point;
import java.util.ArrayList;

import simulator.Environment;
import simulator.PhysicalObject;

import com.grt192.core.SimulatedSensor;

/**
 * Sonar models the behavior of a generic distance sensor (Ultrasonic, IR)
 * providing distance values from object(s) in front of it
 * 
 * @author ajc
 * 
 */
public class Sonar extends SimulatedSensor {

	public static int DISTANCE = 0;

	private static int MAX_RANGE = 150;
	public double mountAngle;

	private final Color color;

	/**
	 * Constructs a new mounted RangeFinder
	 * 
	 * @param env
	 *            Environment where this RangeFinder is located in
	 * @param location
	 *            X Y point position of this sensor
	 * @param mountAngle
	 */
	public Sonar(Environment env, Point location, int mountAngle) {
		super(env, location);
		this.mountAngle = mountAngle;
		color = new Color((float) Math.random(), (float) Math.random(),
				(float) Math.random());
	}

	@Override
	public void poll() {
		setState(DISTANCE, getRange());
	}

	/**
	 * 
	 * @return range data from the simulator environment
	 */
	private double getRange() {
		double range = MAX_RANGE;
		ArrayList<PhysicalObject> known = getPhysicalObjects(MAX_RANGE);

		for (PhysicalObject o : known) {
			// try finding obstacles less than current
			for (int i = 0; i < range; i++) {
				// TODO document this shit
				double x = robotX()
						+ ((x() + i) * (Math.cos(robotHeading() + mountAngle)));
				double y = robotY()
						+ ((y() + i) * (Math.sin(robotHeading() + mountAngle)));
				if (o.intersects(x, y)) {
					range = Math.min(i, range);
					break;
				}
			}

		}
		return range;

	}

	@Override
	public void sensorStateChanged(int id, double prevData, double data) {
		// TODO do event framework
		System.out.println("Sonar: " + data);

	}

	@Override
	public void paint(Graphics g) {
		// TODO document, clean
		g.setColor(Color.BLACK);

		Graphics2D g2d = (Graphics2D) g;
		int width = 3;
		g2d.setStroke(new BasicStroke(width));
		g.drawLine(
				(int) (robotX() + ((x() + 5) * Math.cos(robotHeading()
						+ mountAngle))),
				(int) (robotY() + ((y() + 5) * Math.sin(robotHeading()
						+ mountAngle))),
				(int) (robotX() + ((x() + Math.min(10, getState(DISTANCE))) * Math
						.cos(robotHeading() + mountAngle))),
				(int) (robotY() + ((y() + Math.min(10, getState(DISTANCE))) * Math
						.sin(robotHeading() + mountAngle))));

		width = 1;
		g2d.setStroke(new BasicStroke(width));
		g.setColor(this.color);
		g.drawLine(
				(int) (robotX() + ((x() + Math.min(10, getState(DISTANCE))) * Math
						.cos(robotHeading() + mountAngle))),
				(int) (robotY() + ((y() + Math.min(10, getState(DISTANCE))) * Math
						.sin(robotHeading() + mountAngle))),
				(int) (robotX() + ((x() + getState(DISTANCE)) * Math
						.cos(robotHeading() + mountAngle))),
				(int) (robotY() + ((y() + getState(DISTANCE)) * Math
						.sin(robotHeading() + mountAngle))));
	}

}
